#include "main.h"

/************************************* variable ************************************/
float pitch_data = 0.0;             //俯仰角
int pid_out = 0;                //pid运算输出值
unsigned char direction = 0;        //运动方向
// unsigned char direction1 = 0;        //运动方向
// unsigned char direction2 = 0;        //运动方向
int tx_pid_out = 0.0;
unsigned char motion = 0;
// float objVal = 0.0;             //期望值
// float objspeed1 = 0.0;           //期望速度
// float objspeed2 = 0.0;           //期望速度
/***********************************************************************************/

/*************************************** init **************************************/
pid_pos_typedef pid_construction;
sem_t sem_iic, sem_gpio, sem_pwm;

/***********************************************************************************/

void *uart_task(void *arg)
{
    uart_init();
}
void *led_task(void *arg)
{
    led_init();
}
void *iic_task(void *arg)
{
    i2c_init();
}
void *pwm_task(void *arg)
{
    pwm_init();
}
void *gpio_task(void *arg)
{
    gpio_init();
}
void *pid_set_task(void *arg)
{
    while(1){
        printf("current kp=%f  ki=%f  kd=%f\n\n",pid_construction.kp,pid_construction.ki,pid_construction.kd);
        printf("Here ender kp ki kd:\n");
        scanf("%f%f%f",&pid_construction.kp,&pid_construction.ki,&pid_construction.kd);
        usleep(300*1000);
    }
}
void *socket_task(void *arg)
{
    event_socket_init();
}
/*************************************** main **************************************/

int main(int argc, char* argv[])
{
    int pthread_result = 0;
    pthread_t uart_t, led_t, iic_t, pwm_t, gpio_t, pid_t, ender_t, sock_t;

    pthread_result = pthread_create(&led_t, NULL, led_task, NULL);
    if( pthread_result != 0 ){
        printf("thread led create error!\n");
        exit(-1);
    }
    // pthread_result = pthread_create(&uart_t, NULL, uart_task, NULL);
    // if( pthread_result != 0 ){
    //     printf("thread uart create error!\n");
    //     exit(-1);
    // }
    pthread_result = pthread_create(&iic_t, NULL, iic_task, NULL);
    if( pthread_result != 0 ){
        printf("thread led create error!\n");
        exit(-1);
    }
    pthread_result = pthread_create(&pwm_t, NULL, pwm_task, NULL);
    if( pthread_result != 0 ){
        printf("thread pwm create error!\n");
        exit(-1);
    }
    pthread_result = pthread_create(&gpio_t, NULL, gpio_task, NULL);
    if( pthread_result != 0 ){
        printf("thread gpio create error!\n");
        exit(-1);
    }
    pthread_result = pthread_create(&pid_t, NULL, pid_set_task, NULL);
    if( pthread_result != 0 ){
        printf("thread pid create error!\n");
        exit(-1);
    }
    pthread_result = pthread_create(&ender_t, NULL, pid_set_task, NULL);
    if( pthread_result != 0 ){
        printf("thread pid create error!\n");
        exit(-1);
    }
    // pthread_result = pthread_create(&sock_t, NULL, socket_task, NULL);
    // if( pthread_result != 0 ){
    //     printf("thread socket create error!\n");
    //     exit(-1);
    // }
    
    //进程间信号量 初值
    if( sem_init(&sem_iic, 0, 0) < 0){
        perror("sem_iic error");
        exit(-1);
    }
    if( sem_init(&sem_gpio, 0, 0) < 0){
        perror("sem_gpio error");
        exit(-1);
    }
    if( sem_init(&sem_pwm, 0, 0) < 0){
        perror("sem_pwm error");
        exit(-1);
    }

    pid_pos_init(&pid_construction,370.0,0.0,1.4);

    while(1){
        /*************************************** IIC信号量 **************************************/
        // V操作 信号量+1 告诉iic线程要读取数据
        sem_post(&sem_iic);
        usleep(1*1000);
        // P iic线程读取数据完成
        sem_wait(&sem_iic);
        //俯仰角数据存在 pitch_data
        // printf("pitch  = %f \n",pitch_data);
        // printf("dir1=%d  dir2=%d\n",direction1, direction2);
        // printf("2222222\n");
        /***************************************************************************************/

        /**************************************** PID运算 ***************************************/
        // pid_delta_calc(pid_delta_typedef* pid, float currVal, float objVal);
        pid_out = (int)pid_pos_calc(&pid_construction, pitch_data, 0.0);
        // tx_pid_out = pid_out;
        // printf("pid_out = %d\n",pid_out);
        /***************************************************************************************/
        // if( pid_out >= 0 ){
        //     direction = 0;
        // }
        // else{
        //     direction = 1;
        //     pid_out = (-1)*pid_out;
        // }
        /************************************** GPIO信号量 **************************************/
        // V操作 信号量+1 告诉gpio线程要改变方向
        sem_post(&sem_gpio);
        usleep(1*1000);
        // printf("direction = %d\n",direction);
        // P  -1  gpio线程设置方向完成
        sem_wait(&sem_gpio);
        /***************************************************************************************/

        /*************************************** PWM信号量 **************************************/
        // V操作 信号量+1 告诉pwm线程要改变freq
        sem_post(&sem_pwm);
        usleep(1*1000);
        // P  -1  pwm线程设置freq完成
        sem_wait(&sem_pwm);
        /***************************************************************************************/

        // printf("\n");
        usleep(5*1000);

        
    }

    return 0;
}
/***********************************************************************************/